#!/usr/bin/env python3
#coding=utf-8
## 为了快，未采用严格的约束
"""
   SACP客户端的PYTHON实现
"""

import time
import struct
import binascii
from queue import Queue
import threading
import serial
from typing import List, Set 
from timeit import default_timer as timer
from loguru import logger as log

 
class SacpOpCode:
    OP_READ = 0
    OP_WRITE = 1
    OP_READ_ACK = 2
    OP_WRITE_ACK = 3
    OP_REPORT = 4
    def __init__(self, v = OP_READ):
        self._names = ("READ", "WRITE", "READ_ACK", "WRITE_ACK", "REPORT")
        self._v = v 
    
    def __str__(self):
        if self._v < len(self._names):
            return self._names[self._v]
        return "N/A"
    
    def set(self, v):
        self._v = v 

class SacpType:
    TYPE_BOOL = 0
    TYPE_UINT8 = 1
    TYPE_INT8 = 2
    TYPE_UINT16 = 3
    TYPE_INT16 = 4
    TYPE_UINT32 = 5
    TYPE_INT32 = 6
    TYPE_FLOAT = 7  
    TYPE_DOUBLE = 8
    TYPE_UINT64 = 9
    TYPE_INT64 = 10
    TYPE_OCTET = 11
    TYPE_STATUS = 15
    def __init__(self, v = TYPE_BOOL):
        self._names = ("bool", "uint8", "int8", "uint16", "int16", "uint32", "int32", 
            "float", "double", "uint64", "int64", "octet", "n/a", "n/a", "n/a", "status")
        self._v = v 
    
    def __str__(self):
        if self._v < len(self._names):
            return self._names[self._v]
        return "N/A"
    
    def set(self, v):
        self._v = v     

class Attribute:
    def __init__(self, id = 0, atype = SacpType.TYPE_BOOL, value = None):
        self._id = id
        self._type = atype 
        self._value = value
        
        if self._id > 4095 or self._id < 0:
            self._id = 4095
        
        if not self._value:
            default_values = (0, 0, 0, 0, 0, 0, 0, 
                0.0, 0.0, 0, 0, b'\x00', 0, 0, 0, 0)
            self._value = default_values[self._type]
    
    def __str__(self):
        return "[%04d](%-6s):" % (self._id, SacpType(self._type)) + str(self._value)
        
    def get_id(self):
        return self._id 
    
    def get_type(self):
        return self._type 

    def get_value(self):
        return self._value
    
    def make_id(self):
        return (self._type << 12) | (self._id & 0xfff)
    
    def id_bytes(self)->bytes:
        return struct.pack('< H', self.make_id())
    
    def to_bytes(self)->bytes:
        """转换成字节

        Returns:
            bytes: 返回字节流
        """        
        values = (self.make_id(), self._value)        
        if self._type != SacpType.TYPE_OCTET:
            fmts = ("B", "B", "b", "H", "h", "I", "i", 
                "f", "d", "Q", "q", "s", "s", "s", "s", "B")
            s = struct.Struct('< H ' + fmts[self._type])
            return s.pack(*values)
        else:
            vl = len(self._value)
            if vl >= 128:
                vl = 128
            values = (self.make_id(), vl)
            s = struct.Struct('< H B')
            return s.pack(*values) + self._value[:vl]
        
    def from_bytes(self, data, op_read=False)->int:
        """从字节流中解析一个属性

          先读出2字节的ID，求出类型及id
          然后再解析值 
          仅支持带值的属性，读属性不通过此方法
        Args:
            data (bytes): 输入的字节流 

        Returns:
            int: 返回使用的字节流长度
        """
        if len(data) < 2:
            return 0
        v = struct.unpack('< H', data[:2])
        self._id = v[0] & 0xfff
        self._type = (v[0] >> 12) & 0xf
        self._value = 0
        if self._type == SacpType.TYPE_OCTET:
            self._value = b'\x00'
        
        # 如果是读操作，没有值 
        if op_read:
            return 2
        ## 非读操作，必须至少多一个字节
        if len(data) < 3:
            return 0
        
        if self._type == SacpType.TYPE_OCTET:
            # 第一个字节是长度
            vl = int(data[2])
            self._value = data[3:(3 + vl)]
            return (3 + vl)
        else:
            self._value = b'\x00'
            fmts = ("B", "B", "b", "H", "h", "I", "i", 
                "f", "d", "Q", "q", "s", "s", "s", "s", "B")
            attlens = (1, 1, 1, 2, 2, 4, 4, 
                4, 8, 8, 8, 0, 0, 0, 0, 1)            
            s = struct.Struct('< ' + fmts[self._type])
            ret = s.unpack(data[2:(2 + attlens[self._type])])
            if len(ret) > 0:
                self._value = ret[0] 
            return (2 + attlens[self._type])     

    def dump(self):
        """
        以十六进制形式显示属性值 
        """
        print(binascii.hexlify(self.to_bytes()))        
   

class FrameInfo:
    SACP_SOF = 0xaa
    def __init__(self):
        self.op = SacpOpCode.OP_READ
        self.sequence = 0
        self.priority = 0
        self._attributes:List[Attribute] = []
        
    def set_header(self, op, sequence, priority):
        self.op = op 
        self.sequence = sequence & 0xff
        self.priority = priority & 0xff
    
    def add_attribute(self, attr):
        self._attributes.append(attr)
        
    def add_attributes(self, attrs):
        for v in attrs:
            self._attributes.append(v)        

    def get_attributes(self):
        return self._attributes
 
    def __str__(self):
        s = self.brief()
        for attr in self._attributes:
            s = s + '\r\n + ' + str(attr)
        return s;     

    def brief(self):
        return "OP:%s SEQ:%d PRI:%d ATTRS:%d" % (SacpOpCode(self.op), self.sequence, self.priority, len(self._attributes))
    
    def to_list(self):
        ids = []
        vals = []
        for attr in self._attributes:
            ids.append(attr.get_id())
            vals.append(attr.get_value())
        return ids,vals

    def crc16(self, init, data):
        tables = (0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
            0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400)
        crc = init 
        for v in data:
            crc = tables[(v ^ crc) & 15]^(crc >> 4)
            crc = tables[((v >> 4) ^ crc) & 15]^(crc >> 4)
        return crc & 0xffff
    
    def to_frame(self):
        data = bytes()
        if self.op == SacpOpCode.OP_READ:
            for attr in self._attributes:
                data = data + attr.id_bytes()
        else:
            for attr in self._attributes:
                data = data + attr.to_bytes()

        control = ((self.priority << 6) & 0xc0) + (self.op & 0x7)

        s = struct.Struct('B B B B')
        header_values = (self.SACP_SOF, self.sequence, control, len(data))
        header = s.pack(*header_values)
        hdrsum = 0
        for b in header:
            hdrsum = hdrsum + b
        hdrsum = ((~hdrsum) + 1) & 0xff
        header = header + struct.pack('B', hdrsum)
        crc = self.crc16(0xffff, header + data)
        return header + data + struct.pack("< H", crc) 
    
    def dump(self):
        """
        以十六进制形式显示属性值 
        """
        print(binascii.hexlify(self.to_frame()))      

    def check_header(self, hdr:bytes)->bool:
        if len(hdr) < 1:
            return False 
        if hdr[0] != self.SACP_SOF:
            #print("invalid sof")
            return False 
        if len(hdr) < 5:
            return True 
        hdrsum = 0
        for b in hdr[:4]:
            hdrsum = hdrsum + b
        hdrsum = ((~hdrsum) + 1) & 0xff
        
        if hdrsum != hdr[4]:
            #print("invalid header sum")
            return False 
        return True
    
    def check_frame(self, frame:bytes)->bool:
        """检查帧是否有效

        Args:
            frame (bytes): 输入的帧数据 

        Returns:
            bool: 如果为真
        """
        #print(binascii.hexlify(frame))
        size = len(frame)
        if size < 5:
            log.warning("frame check: size too small")
            return False 
        if not self.check_header(frame):
            log.warning("frame check: invalid header")
            return False 
        data_size = int(frame[3])
        if data_size != len(frame[5:-2]):
            log.warning("frame check: invalid data size")
            return False
        # check crc16
        crc_pkt = struct.unpack('< H', frame[-2:])
        crc = self.crc16(0xffff, frame[:-2])
        if crc_pkt[0] != crc:
            log.warning("frame check: crc mismatch, calc:%04x pkt:%04x" % (crc, crc_pkt[0]))
            return False 
        return True 
        
    def from_frame(self, frame:bytes)->bool:
        # 'aa0000094d6420f4645078563412375'  
        if not self.check_frame(frame):
            return False 
        s = struct.Struct('B B B B B')
        hdr = s.unpack(frame[:5])        
        #print(hdr)
        #sof = hdr[0]
        sequence = hdr[1]
        op = hdr[2] & 0x7
        pri = (hdr[2] >> 6) & 0x3
        #data_size = hdr[3]
        
        # check data size if correct
        data = frame[5:-2]
        # if len(data) != data_size:
        #     print("invalid frame, data size error:(hdr %d: frame %d)" % (data_size, len(data)))
        #     return False
        
        base = 0
        attrs = []   
        op_read = False     
        if op == SacpOpCode.OP_READ:
            op_read = True
        
        #print(SacpOpCode(op))
        left = len(data)
        #print(binascii.hexlify(data))

        while left > 0:
            a = Attribute()
            alen = a.from_bytes(data[base:], op_read)
            if alen == 0:
                log.warning("parse frame attributes failed")
                return False
            base = base + alen
            left = left - alen
            #print("alen left ", alen, ",", left)
            #print(a)
            attrs.append(a)
        # parse ok
        self.op = op 
        self.priority = pri 
        self.sequence = sequence
        self._attributes.clear()
        for a in attrs:
            self._attributes.append(a)
        return True 


class SacpClient:
    """SACP客户端对象
    """
    def __init__(self, serial_port:str, serial_rate:int, report_queue_size=1000, debug=False):
        self.serial = serial_port
        self.serial_rate = serial_rate
        self.serial_handle = None
        self.tx_queue = Queue()
        self.reports = Queue(report_queue_size)
        self.rx_responses = Queue()
        self.tx_responses = Queue()
        self._rx_thread = threading.Thread(target=self.__rx_thread_main)
        self._tx_thread = threading.Thread(target=self.__tx_thread_main)
        self.running = False 
        # 接收过程
        self.rx_buffer = []
        self.rx_size = 0
        self.rx_expired = timer()
        self.req_sequence = 0
        self.report_queue_full = False
        self.raw_debug = debug

    def open_serial(self, port:str, rate:int):
        """打开指定的串口

        Args:
            port (str): 端口名称
            rate (int): 波特率

        Returns:
            _type_: 如果失败，返回None，如果成功，返回串口的handle
        """
        try:
            s = serial.Serial(port, rate, timeout = 0)        
            return s
        except Exception as e:    
            return None 
            
    def __rx_thread_main(self):
        log.info("rx thread start")
        
        if not self.serial_handle:
            log.error("internal error, serial port not opened")
            return 
        
        dev = self.serial_handle
        self.running = True 
        while self.running:
            ## 如果串口有数据，接收到缓存
            if dev.in_waiting > 0:
                data = dev.read(dev.in_waiting)
                # if self.raw_debug:
                #     print("rx:", binascii.hexlify(data))
                while len(data) > 0:                    
                    data = self._receive_input(data)
            #time.sleep(0.01)       
        # 关闭串口
        dev.close()            
        log.info("rx thread exit")

    def __tx_thread_main(self):
        log.info("tx thread start")        
        if not self.serial_handle:
            log.error("internal error, serial port not opened")
            return 
         
        while self.running:
            try:
                fi = self.tx_queue.get(timeout=0.1)            
                data = fi.to_frame()
                if len(data):
                    self.serial_handle.write(data)
                    if self.raw_debug:
                        log.debug("tx frame >>> " + fi.brief())
                        log.debug("tx:" + str(binascii.hexlify(data)))
            except Exception as e:
                fi = None 
            #time.sleep(0.01)
        
        log.info("tx thread exit")


    def start(self)->bool:
        if not self.running:
            ## 初始化 
            self.serial_handle = self.open_serial(self.serial, self.serial_rate)
            if not self.serial_handle:
                log.error("open serial port '{port}' failed".format(port=self.serial))
                return False        
            self._rx_thread.start()
            self._tx_thread.start()
            return True 
        else:
            return False

    def stop(self):
        if self.running:
            self.running = False 
            self._rx_thread.join()
            self._tx_thread.join()
    
    def get_report(self, timeout=0)->FrameInfo:
        """获取收到的REPORT帧

        Args:
            timeout (int, optional): 等待超时时间. Defaults to 0.

        Returns:
            FrameInfo: 返回一个帧对象实例
        """
        ret = None
        try:
            ret = self.reports.get(timeout=timeout)
        except Exception as e:
            return None 
        return ret 

    def _put_report(self, v:FrameInfo):
        try:
            self.reports.put(v, block=False)
            self.report_queue_full = False
        except Exception as e:
            if not self.report_queue_full:
                log.warning("queue of report is full")
                self.report_queue_full = True 

    def _get_read_response(self, timeout=0)->FrameInfo:
        ret = None
        try:
            ret = self.rx_responses.get(timeout=timeout)
        except Exception as e:
            return None 
        return ret         
 
    def _get_write_response(self, timeout=0)->FrameInfo:
        ret = None
        try:
            ret = self.tx_responses.get(timeout=timeout)
        except Exception as e:
            return None 
        return ret    
 
    def _receive_input(self, data:bytes)->bytes:
        """输入读入的字节流，处理分为三个状态         
         1.当接收字节为0时， 查找SOF，找到SOF后，设置RX SIZE 1
         2.当接收字节小于头部时，接收到头部大小后，检查头部情况，如果检查头部失败，返回SOF以后的数据
         3.当接收字节大于头部时，读出帧数据大小，接收完成后，校验帧是否正确，如果帧失败，丢弃该帧，返回剩余字节。
         
         接收超时，接收到第一个SOF时，更新超时标志，如果超过指定时间没有新数据，重置 
         
        Args:
            data (int): _description_

        Returns:
            bytes: 返回剩余的数据 
        """
        if len(data) == 0:
            return bytes() 
        
        data_len = len(data)
        # 超时逻辑
        now = timer()
        if self.rx_size > 0 and now > self.rx_expired:
            rxdata = bytes(self.rx_buffer)
            log.warning("rx timeout(len=%d) %s" % (self.rx_size, binascii.hexlify(rxdata)))            
            self.rx_size = 0
            self.rx_buffer.clear()
        
        data_offset = 0
        # 逻辑1 还没有收到过数据，等待SOF
        if self.rx_size == 0:
            while data_offset < data_len:                
                if data[data_offset] == FrameInfo.SACP_SOF:
                    self.rx_size = 1
                    self.rx_buffer.append(data[data_offset])
                    self.rx_expired = timer() + 0.5 # 系统有时会缓存比较长时间的数据,奇怪 
                    data_offset = data_offset + 1
                    break
                data_offset = data_offset + 1
        
            # 所有数据没有找到SOF，直接返回
            if self.rx_size == 0:
                return bytes() 
        
        # 逻辑2 如果接收到头部位置
        if self.rx_size < 5:
            # 只读完头部即可
            while self.rx_size < 5 and data_offset < data_len:
                self.rx_size = self.rx_size + 1
                self.rx_buffer.append(data[data_offset])
                data_offset = data_offset + 1
                
            # 如果未接收完数据，说明数据已用完，直接返回
            if self.rx_size < 5:
                return bytes()

            ## 接收完头部，看看是否正确
            hdr = bytes(self.rx_buffer)
            if not FrameInfo().check_header(hdr):
                self.rx_size = 0
                self.rx_buffer.clear()
                ## 如果接收到异常的数据，需要从已接收数据中查找SOF, 返回除头部外的SOF外的所有数据+剩余数据
                return hdr[1:] + data[data_offset:]
                            
        # 逻辑3 到了这里，表示接收的头部数据正确
        # 求出整包的大小， 数据大小在第三字节
        frame_size = 5 + self.rx_buffer[3] + 2 
        while self.rx_size < frame_size and data_offset < data_len:
            self.rx_size = self.rx_size + 1
            self.rx_buffer.append(data[data_offset])
            data_offset = data_offset + 1            

        ## 看看是否接收完成
        if self.rx_size < frame_size:
            # 未接收完成，返回数据空
            return bytes()
        
        # 接收完成， 校验整包数据是否正常
        if self.raw_debug:
            log.debug("rx:" + str(binascii.hexlify(bytes(self.rx_buffer))))
        
        f = FrameInfo()
        if f.from_frame(bytes(self.rx_buffer)):
            if self.raw_debug:
                log.debug("rx frame <<< " + f.brief())
            if f.op == SacpOpCode.OP_REPORT:
                self._put_report(f)
            elif f.op == SacpOpCode.OP_READ_ACK:
                self.rx_responses.put(f)
            elif f.op == SacpOpCode.OP_WRITE_ACK:
                self.tx_responses.put(f)
        
        ## 重置接收状态
        self.rx_size = 0
        self.rx_buffer.clear()            
        
        # 如果还有字节，返回剩余数据 
        return data[data_offset:]
    
    def read_attributes(self, attrs:List[Attribute], priority=0, timeout=0.5)->List[Attribute]:
        """读一组属性

        Args:
            attrs (List[Attribute]): 属性列表
            priority (int, optional): 优先级. Defaults to 0.
            timeout (float, optional): 等待时间. Defaults to 0.5.

        Returns:
            List[Attribute]: 返回读出的属性列表
        """
        fi = FrameInfo()
        seq = self.req_sequence & 0xff
        self.req_sequence += 1
        fi.set_header(SacpOpCode.OP_READ, seq, priority)        
        fi.add_attributes(attrs)
        self.tx_queue.put(fi)
        # 等待响应返回
        now = timer()
        expired = now + timeout
        while now < expired:
            fi = self._get_read_response(0.1)
            if fi :
                if fi.sequence == seq:
                    # 短时间内收到相同的SEQ的响应报文，应该是该回应
                    return fi.get_attributes()
                else:
                    log.warning("read rx unexpected seq frame: rx:%d, expect:%d" % (fi.sequence, seq))    
            now = timer()
        log.warning("read timeout, seq=%d" % seq)
        return None

    def write_attributes(self, attrs:List[Attribute], priority=0, timeout=0.5)->List[Attribute]:
        """写一组属性值 

        Args:
            attrs (List[Attribute]): 属性列表
            priority (int, optional): 优先级. Defaults to 0.
            timeout (float, optional): 写响应超时. Defaults to 0.5.

        Returns:
            List[Attribute]: 返回的属性列表
        """
        fi = FrameInfo()
        seq = self.req_sequence & 0xff
        self.req_sequence += 1
        fi.set_header(SacpOpCode.OP_WRITE, seq, priority)
        fi.add_attributes(attrs)
        self.tx_queue.put(fi)        
        # 等待响应返回
        now = timer()
        expired = now + timeout
        while now < expired:
            fi = self._get_write_response(0.1)
            if fi:
                if fi.sequence == seq:
                    # 短时间内收到相同的SEQ的响应报文，应该是该回应
                    return fi.get_attributes()
                else:
                    log.warning("write rx unexpected seq frame: rx%d, expect:%d" % (fi.sequence, seq))    
            now = timer()
        log.warning("write timeout, seq=%d" % seq)
        return None
